#include "FirstPersonCameraController.h"

FirstPersonCameraController::FirstPersonCameraController(Ogre::SceneManager* sceneManager, Ogre::Real moveSpeed, Ogre::Real rotationSpeed)
{
	this->sceneManager = sceneManager;

	cameraNode = sceneManager->getRootSceneNode()->createChildSceneNode();
	cameraYawNode = cameraNode->createChildSceneNode();
	cameraPitchNode = cameraYawNode->createChildSceneNode();
	cameraRollNode = cameraPitchNode->createChildSceneNode();

	cameraTranslation = Ogre::Vector3::ZERO;
    cameraMoveSpeed = moveSpeed;
	cameraRotationSpeed = Ogre::Degree(rotationSpeed);
    cameraRotationX = 0.0;
    cameraRotationY = 0.0;
}

void FirstPersonCameraController::update(OIS::Keyboard* keyboard, OIS::Mouse* mouse, unsigned long lastFrameDuration)
{
	cameraTranslation = Ogre::Vector3::ZERO;
	Ogre::Real realCameraMoveSpeed = cameraMoveSpeed * ((float)lastFrameDuration / 1000.0f);

    if(keyboard->isKeyDown(OIS::KC_LSHIFT))
    {
        realCameraMoveSpeed *= 10.0;
    }

	if(keyboard->isKeyDown(OIS::KC_RSHIFT))
    {
        realCameraMoveSpeed *= 10.0;
    }

    if(keyboard->isKeyDown(OIS::KC_PGUP))
    {
        cameraTranslation.y += realCameraMoveSpeed;
    }

    if(keyboard->isKeyDown(OIS::KC_PGDOWN))
    {
        cameraTranslation.y -= realCameraMoveSpeed;
    }

    if(keyboard->isKeyDown(OIS::KC_W))
    {
        cameraTranslation.z -= realCameraMoveSpeed;
    }

    if(keyboard->isKeyDown(OIS::KC_S))
    {
        cameraTranslation.z += realCameraMoveSpeed;
    }

    if(keyboard->isKeyDown(OIS::KC_A))
    {
        cameraTranslation.x -= realCameraMoveSpeed;
    }

    if(keyboard->isKeyDown(OIS::KC_D))
    {
        cameraTranslation.x += realCameraMoveSpeed;
    }

    if(keyboard->isKeyDown(OIS::KC_Q))
    {
		cameraRotationX += cameraRotationSpeed;
    }

    if(keyboard->isKeyDown(OIS::KC_E))
    {
		cameraRotationX -= cameraRotationSpeed;
    }

    if(keyboard->isKeyDown(OIS::KC_O))
    {
		cameraRotationX = 0;
		cameraRotationY = 0;
    }

    const OIS::MouseState &ms = mouse->getMouseState();

    if(ms.buttonDown(OIS::MB_Right))
    {
        cameraTranslation.x += ms.X.rel * realCameraMoveSpeed;
        cameraTranslation.y -= ms.Y.rel * realCameraMoveSpeed;
    }
    else
    {
		cameraRotationX = Ogre::Degree(-ms.X.rel * cameraRotationSpeed);
		cameraRotationY = Ogre::Degree(-ms.Y.rel * cameraRotationSpeed);
    }

	Ogre::Real pitchAngle;
    Ogre::Real pitchAngleSign;

    cameraYawNode->yaw(cameraRotationX);
    cameraPitchNode->pitch(cameraRotationY);

    cameraNode->translate(cameraYawNode->getOrientation() * cameraPitchNode->getOrientation() * cameraTranslation, Ogre::SceneNode::TS_LOCAL);

    pitchAngle = (2 * Ogre::Degree(Ogre::Math::ACos(cameraPitchNode->getOrientation().w)).valueDegrees());

    pitchAngleSign = cameraPitchNode->getOrientation().x;

    if(pitchAngle > 90.0f)
    {
        if(pitchAngleSign > 0)
        {
            cameraPitchNode->setOrientation(Ogre::Quaternion(Ogre::Math::Sqrt(0.5f), Ogre::Math::Sqrt(0.5f), 0, 0));
        }
        else if (pitchAngleSign < 0)
        {
            cameraPitchNode->setOrientation(Ogre::Quaternion(Ogre::Math::Sqrt(0.5f), -Ogre::Math::Sqrt(0.5f), 0, 0));
        }
    }
}

void FirstPersonCameraController::setPosition(Ogre::Vector3 position)
{
	cameraNode->setPosition(position);
}

void FirstPersonCameraController::getPositionOrientation(Ogre::Vector3& position, Ogre::Quaternion& orientation)
{
	position = cameraNode->getPosition();
	orientation = cameraYawNode->getOrientation() * cameraPitchNode->getOrientation() * cameraRollNode->getOrientation();
}